



#include <Voxelization.h>

#include "GeometryUtils.h"
#include <DrawUtils.h>

#include <assert.h>


Voxelization::Voxelization(const Model& referenceModel, int gridResolution, float weightMargin, int smoothingIterations) : 

m_referenceModel(referenceModel), 
m_gridResolution(gridResolution), 
m_weightMargin(weightMargin), 
m_smoothingIterations(smoothingIterations)

{
	
}


static inline float _rgbToHue(float r, float g, float b)
{
	if ( fabs(r-g)<1.0e-3f && fabs(g-b)<1.0e-3f) 
	{
		// If all color values are equal, just return the saturation instead of the hue. This makes greyscale maps work too.
		return (r+g+b)/3.f;
	}
	
	float alpha = 2.f*r - g - b;
	float beta = sqrtf(3.0f) * (g-b);
	return 0.5f + atan2f(beta, alpha) / (2.f * M_PI);
}


bool _compareVoxelWeights(const Voxelization::Voxel& first, const Voxelization::Voxel& second)
{
	return first.m_weight < second.m_weight;
}


void Voxelization::buildFractureGeometries(vector<Geometry>& geometries)
{
	printf("Loaded reference mesh with %d vertices\n", (int)m_referenceModel.m_geometry.m_vertices.size());
	
	// Build mesh kdTree for fast intersection queries
	printf("Building mesh kd-tree...\n");
	m_meshKdTree = new KdTree(m_referenceModel.m_geometry);
	
	printf("Initializing voxel grid...\n");
	
	// Bounding box of model defines the boundary of a uniform grid or 'cage' of voxels
	m_meshAabb = m_referenceModel.m_geometry.computeAabb();
	
	Vec4 originalCageDiag = m_meshAabb.m_maxima - m_meshAabb.m_minima;
	float maxVal;
	int maxExtent = originalCageDiag.maxIndex(maxVal);
	int minorExtents[2] = { (maxExtent+1)%3, (maxExtent+2)%3 };
	
	// Make voxels roughly cubic
	m_cageResolution[maxExtent] = m_gridResolution;
	m_cageResolution[minorExtents[0]] = int( float(m_gridResolution) * originalCageDiag[minorExtents[0]] / originalCageDiag[maxExtent] );
	m_cageResolution[minorExtents[1]] = int( float(m_gridResolution) * originalCageDiag[minorExtents[1]] / originalCageDiag[maxExtent] );
	if (m_cageResolution[minorExtents[0]]<1) m_cageResolution[minorExtents[0]]=1;
	if (m_cageResolution[minorExtents[1]]<1) m_cageResolution[minorExtents[1]]=1;
	
	// Expand AABB by three voxel widths on either side, to avoid issues with mesh triangles touching the side of the voxel grid.
	Vec4 expand = m_meshAabb.m_maxima - m_meshAabb.m_minima;
	expand[0] *= 3.f/(float)m_cageResolution[0];
	expand[1] *= 3.f/(float)m_cageResolution[1];
	expand[2] *= 3.f/(float)m_cageResolution[2];
	m_meshAabb.m_maxima += expand;
	m_meshAabb.m_minima -= expand;
	
	// Compute final voxel dimensions
	Vec4 expandedCageDiag = m_meshAabb.m_maxima - m_meshAabb.m_minima;
	
	assert(m_cageResolution[0]>0); m_voxelEdges[0] = expandedCageDiag[0] / m_cageResolution[0]; 
	assert(m_cageResolution[1]>0); m_voxelEdges[1] = expandedCageDiag[1] / m_cageResolution[1];
	assert(m_cageResolution[2]>0); m_voxelEdges[2] = expandedCageDiag[2] / m_cageResolution[2];
	
	// Create a voxelization of the reference mesh which embeds it as closely as possible
	generateVoxelization();
	
	// Now generate fracture pieces, which each consist of a subset of the voxels.
	// Fist, create seeds for fracture pieces by querying mesh painted colors and assigning them to the boundary voxels.
	printf("Generate seeds for fracture pieces...\n");
	generateFractureSeeds();
		
	// Create the voxels subsets for each fracture piece by growing the seeds.
	buildFracturePieces();
	
	// We now have fragments each consisting of a subset of voxels, which together fill the mesh voxelization.
	// Generate a triangle mesh from each one of these fragments.
	generateMeshes();
	
	// Create an adjacency structure for each piece before further processing
	for (int i=0; i<m_fracturePieces.size(); ++i)
	{
		FracturePiece& piece = m_fracturePieces[i];
		printf("Building vertex neighbourhoods for fracture piece %d/%d...\n", i, (int)m_fracturePieces.size());
		piece.buildVertexNeighbourhoods();
	}
	
	// Apply mesh smoothing algorithm to each fracture piece (thus modifying the lattice vertices).
	// Iterate for a reasonable number of times.
	{
		int nSmoothIters=0;
		while (nSmoothIters < m_smoothingIterations)
		{
			printf("Smoothing iteration %d/%d...\n", nSmoothIters+1, m_smoothingIterations);
			for (int i=0; i<m_fracturePieces.size(); ++i)
			{
				FracturePiece& piece = m_fracturePieces[i];
				piece.smooth(*this);
			}
			nSmoothIters++;
		}
	}
	
	// Now snap the outer surface to the closest reference mesh triangles, and project the mesh UVs onto the fracture pieces.
	printf("Snapping voxelization boundary to original mesh surface...\n");
	projectOntoReferenceMesh();
		
	// Generate a full Geometry per fracture piece with all the vertex, normal & UV data.
	for (int i=0; i<m_fracturePieces.size(); ++i)
	{
		printf("Finalizing geometry of fracture piece %d/%d.\n", i, (int)m_fracturePieces.size());
		FracturePiece& piece = m_fracturePieces[i];
		piece.finalizeGeometry(*this);
	}
	
	// Create separate geometries for disconnected chunks of each fracture piece
	for (int i=0; i<m_fracturePieces.size(); ++i)
	{
		printf("Disconnecting fracture piece %d/%d.\n", i+1, (int)m_fracturePieces.size());
		FracturePiece& piece = m_fracturePieces[i];
		piece.disconnect(geometries);
	}
	
	// Finally, repair interior/boundary interfaces of each geometry
	printf("Repairing interfaces...\n");
	for (int i=0; i<geometries.size(); ++i)
	{
		FracturePiece::repairInterfaces(geometries[i]);
	}
	
	int numVerts=0;
	for (int i=0; i<geometries.size(); ++i)
	{
		numVerts += geometries[i].m_vertices.size();
	}
	
	printf("\nFracture piece generation complete - created %d pieces with %d vertices in total.\n", (int)geometries.size(), numVerts);
}



void Voxelization::generateVoxelization()
{
	// Make master voxel grid
	for (int ci=0; ci<m_cageResolution[0]; ++ci)
	{
		for (int cj=0; cj<m_cageResolution[1]; ++cj)
		{
			for (int ck=0; ck<m_cageResolution[2]; ++ck)
			{
				int voxelIndex = _voxelIndex(ci, cj, ck);
				Voxel voxel(voxelIndex);
				
				m_masterGrid.push_back(voxel);
				//m_masterGrid.insert(map<int, Voxel>::value_type(voxelIndex, voxel));
			}
		}
	}
	
	// Make master vertex grid
	for (int vi=0; vi<=m_cageResolution[0]; ++vi)
	{
		for (int vj=0; vj<=m_cageResolution[1]; ++vj)
		{
			for (int vk=0; vk<=m_cageResolution[2]; ++vk)
			{
				float x = m_meshAabb.m_minima[0] + m_voxelEdges[0] * vi;
				float y = m_meshAabb.m_minima[1] + m_voxelEdges[1] * vj;
				float z = m_meshAabb.m_minima[2] + m_voxelEdges[2] * vk;
				Vec4 P(x,y,z);
				
				m_masterVertices.push_back(P);
				
				//int latticeIndex = _latticeIndex(vi, vj, vk);
				//m_masterVertices.insert(map<int, Vec4>::value_type(latticeIndex, P));
			}
		}
	}
	
	// Mark voxels which are touched by the AABB of some mesh triangle as active.
	printf("Find voxels intersected by mesh...\n");
	for (int ti=0; ti<m_referenceModel.m_geometry.m_triangles.size(); ++ti)
	{
		const Geometry::Triangle& t = m_referenceModel.m_geometry.m_triangles[ti];
		
		const Vec4& A = m_referenceModel.m_geometry.m_vertices[ t.m_vertex[0] ];
		const Vec4& B = m_referenceModel.m_geometry.m_vertices[ t.m_vertex[1] ];
		const Vec4& C = m_referenceModel.m_geometry.m_vertices[ t.m_vertex[2] ];
		
		Aabb triAabb;
		triAabb.reset();
		triAabb.includePoint(A);
		triAabb.includePoint(B);
		triAabb.includePoint(C);
		
		int ciMin, cjMin, ckMin;
		int ciMax, cjMax, ckMax;
		
		_getVoxelContaining(triAabb.m_minima, ciMin, cjMin, ckMin);
		_getVoxelContaining(triAabb.m_maxima, ciMax, cjMax, ckMax);
		
		for (int ci=ciMin; ci<=ciMax; ++ci) 
			for (int cj=cjMin; cj<=cjMax; ++cj) 
				for (int ck=ckMin; ck<=ckMax; ++ck) 
				{
					int voxelIndex = _voxelIndex(ci, cj, ck);
					
					Voxel& voxel = m_masterGrid[voxelIndex];
					voxel.m_active = true;
				}
	}
	
	// Of those voxels which touch some triangle AABB, strip those which do not actually intersect some triangle.
	// This gives the "base" voxels, which embed the reference mesh, and should be reasonably close to it.
	for (int voxelIndex=0; voxelIndex<m_masterGrid.size(); ++voxelIndex)
	{
		Voxel& voxel = m_masterGrid[voxelIndex];
		if (!voxel.m_active) continue;
		
		int ci, cj, ck;
		_voxelCoords(voxelIndex, ci, cj, ck);
		Aabb voxelAabb = _computeVoxelAabb(ci, cj, ck);
		Vec4 voxelCenter = 0.5f * (voxelAabb.m_minima + voxelAabb.m_maxima);
		Vec4 voxelHalfExtents = 0.5f * (voxelAabb.m_maxima - voxelAabb.m_minima);
		
		// Find potentially colliding triangles
		vector<int> triangleIndices;
		m_meshKdTree->intersect(voxelAabb, triangleIndices);
		
		// Do narrowphase voxel-triangle tests.
		voxel.m_active = false;
		for (int ti=0; ti<triangleIndices.size(); ++ti)
		{
			int triIndex = triangleIndices[ti];
			const Geometry::Triangle& triangle = m_referenceModel.m_geometry.m_triangles[triIndex];
			const Vec4& A = m_referenceModel.m_geometry.m_vertices[triangle.m_vertex[0]];
			const Vec4& B = m_referenceModel.m_geometry.m_vertices[triangle.m_vertex[1]];
			const Vec4& C = m_referenceModel.m_geometry.m_vertices[triangle.m_vertex[2]];
			
			// Collide triangle(A, B, C) with voxel AABB 			
			float triverts[3][3] = { {A[0], A[1], A[2]}, {B[0], B[1], B[2]}, {C[0], C[1], C[2]} };
			
			if ( GeometryUtils::triBoxOverlap(&voxelCenter[0], &voxelHalfExtents[0], triverts) )
			{
				voxel.m_active = true;
				break;
			}
		}
	}
	
	// Assuming mesh is closed, m_voxels is now a closed shell. Flood fill the interior.
	// First, go through voxels on boundary of grid, finding ones that are not active, and flood fill starting from these. The filled cells
	// are then known to be "exterior" to the shell. (Actually, we ensured that the boundary voxels don't touch the mesh, so this is overkill... anyway).
	
	vector<int> inactiveBoundaryVoxels;
	
	int resX = m_cageResolution[0]-1;
	int resY = m_cageResolution[1]-1;
	int resZ = m_cageResolution[2]-1;
	
	// XY
	for (int ci=0; ci<m_cageResolution[0]; ++ci)
		for (int cj=0; cj<m_cageResolution[1]; ++cj)
		{	
			_addBoundaryVoxel(ci, cj, 0, inactiveBoundaryVoxels);
			_addBoundaryVoxel(ci, cj, resZ, inactiveBoundaryVoxels);
		}
	
	// YZ
	for (int cj=0; cj<m_cageResolution[1]; ++cj)
		for (int ck=0; ck<m_cageResolution[2]; ++ck)
		{
			_addBoundaryVoxel(0, cj, ck, inactiveBoundaryVoxels);
			_addBoundaryVoxel(resX, cj, ck, inactiveBoundaryVoxels);
		}
	
	// ZX
	for (int ck=0; ck<m_cageResolution[2]; ++ck)
		for (int ci=0; ci<m_cageResolution[0]; ++ci)
		{
			_addBoundaryVoxel(ci, 0, ck, inactiveBoundaryVoxels);
			_addBoundaryVoxel(ci, resY, ck, inactiveBoundaryVoxels);
		}
	
	floodFillExteriorVoxels(inactiveBoundaryVoxels);
	
	// Now we can just mark all non-exterior voxels as active
	m_numActiveVoxels = 0;
	for (int voxelIndex=0; voxelIndex<m_masterGrid.size(); ++voxelIndex)
	{
		Voxel& voxel = m_masterGrid[voxelIndex];
		if (!voxel.m_exterior) voxel.m_active = true;
		
		if (voxel.m_active) m_numActiveVoxels++;
	}
	
	// Once flood fill is done, we need to find vertices which lie outside the mesh because they will later be snapped to the reference mesh.
	
	// There may be active voxels which have corners outside the mesh, which are NOT on the boundary of the voxels embedding the mesh (e.g. in concave areas).
	// These also must be snapped to the mesh. We identify them here, by pushing (extruding) mesh triangles out by a few voxel widths, 
	// defining a shell around the mesh, and finding all voxel corners (lattice points) lying inside this shell.
	
	// Find active voxels on boundary
	printf("Find exterior lattice points ...\n");
	
	float extrusionLength = 2.5f * max(max(m_voxelEdges[0], m_voxelEdges[1]), m_voxelEdges[2]);
	
	for (int ti=0; ti<m_referenceModel.m_geometry.m_triangles.size(); ++ti)
	{
		// Construct extruded triangle (extrude outward along per-vertex normals).
		const Geometry::Triangle& t = m_referenceModel.m_geometry.m_triangles[ti];
		
		const Vec4& A = m_referenceModel.m_geometry.m_vertices[ t.m_vertex[0] ];
		const Vec4& B = m_referenceModel.m_geometry.m_vertices[ t.m_vertex[1] ];
		const Vec4& C = m_referenceModel.m_geometry.m_vertices[ t.m_vertex[2] ];
		
		const Vec4& NA = m_referenceModel.m_geometry.m_smoothedNormals[ t.m_vertex[0] ];
		const Vec4& NB = m_referenceModel.m_geometry.m_smoothedNormals[ t.m_vertex[1] ];
		const Vec4& NC = m_referenceModel.m_geometry.m_smoothedNormals[ t.m_vertex[2] ];
		
		Vec4 Navg = 0.3333333f * (NA + NB + NC);
		
		Vec4 planeEqn = (B - A) % (C - A);
		planeEqn.normalizeIfNotZero();
		if ( Navg * planeEqn < 0.0f ) planeEqn *= -1.f;
		
		GeometryUtils::computePlaneEquation(planeEqn, A);
		
		Vec4 Ae = A + extrusionLength*NA;
		Vec4 Be = B + extrusionLength*NB;
		Vec4 Ce = C + extrusionLength*NC;
		
		Aabb extrudedTriAABB;
		extrudedTriAABB.includePoint(A);
		extrudedTriAABB.includePoint(B);
		extrudedTriAABB.includePoint(C);
		extrudedTriAABB.includePoint(Ae);
		extrudedTriAABB.includePoint(Be);
		extrudedTriAABB.includePoint(Ce);
		
		// Find (active) voxels which overlap the AABB of the extruded triangle
		int ciMin, cjMin, ckMin;
		int ciMax, cjMax, ckMax;
		
		_getVoxelContaining(extrudedTriAABB.m_minima, ciMin, cjMin, ckMin);
		_getVoxelContaining(extrudedTriAABB.m_maxima, ciMax, cjMax, ckMax);
		
		for (int ci=ciMin; ci<=ciMax; ++ci) 
			for (int cj=cjMin; cj<=cjMax; ++cj) 
				for (int ck=ckMin; ck<=ckMax; ++ck) 
				{
					int voxelIndex = _voxelIndex(ci, cj, ck);
					Voxel& voxel = m_masterGrid[voxelIndex];
					if (!voxel.m_active) continue;
					
					// For each lattice point of the active voxel, do full test of whether point is in extrusion region.
					for (int corner=0; corner<8; ++corner)
					{
						int vi = ci + ((corner >> 0) & 1);
						int vj = cj + ((corner >> 1) & 1);
						int vk = ck + ((corner >> 2) & 1);
						Vec4 P = _latticeVertex(vi, vj, vk);
						
						float sd = planeEqn*P + planeEqn[3];
						
						if ( sd>=0.0f && sd<=extrusionLength )
						{
							Vec4 sliceA = A + NA * sd / (planeEqn * NA);
							Vec4 sliceB = B + NB * sd / (planeEqn * NB);
							Vec4 sliceC = C + NC * sd / (planeEqn * NC);
							
							float w[3];
							GeometryUtils::getProjectedBarycoords(P, sliceA, sliceB, sliceC, w);
							
							if (w[0]>=-0.001f && w[0]<=1.001f &&
								w[1]>=-0.001f && w[1]<=1.001f &&
								w[2]>=-0.001f && w[2]<=1.001f )
							{
								// This corner vertex is on boundary
								int latticeIndex = _latticeIndex(vi, vj, vk);
								
								// Add it to set if not already added
								if (m_boundaryVertices.count(latticeIndex) == 0) 
								{
									m_boundaryVertices.insert(latticeIndex);
								}
							}
						}
					}
				}
	}
}


inline void Voxelization::_addBoundaryVoxel(int ci, int cj, int ck, vector<int>& inactiveBoundaryVoxels)
{
	int voxelIndex = _voxelIndex(ci, cj, ck);
	
	Voxelization::Voxel& voxel = m_masterGrid[voxelIndex];
	if (!voxel.m_active) 
	{
		voxel.m_exterior = true;
		inactiveBoundaryVoxels.push_back(voxelIndex);
	}
}

inline void Voxelization::_testNeighbour_floodFillExteriorVoxels(int ni, int nj, int nk, std::queue<int>& Q)
{
	if (ni<0 || ni>=m_cageResolution[0]) return;
	if (nj<0 || nj>=m_cageResolution[1]) return;
	if (nk<0 || nk>=m_cageResolution[2]) return;
	
	int neighbourVoxelIndex = _voxelIndex(ni, nj, nk);
	Voxelization::Voxel& neighbourvoxel = m_masterGrid[neighbourVoxelIndex];
	
	if (!neighbourvoxel.m_exterior && !neighbourvoxel.m_active) 
	{	
		neighbourvoxel.m_exterior = true;
		Q.push(neighbourVoxelIndex);
	}
}

void Voxelization::floodFillExteriorVoxels(const vector<int>& seedExteriorVoxels)
{
	printf("Flood filling exterior voxels...\n");
	
	std::queue<int> Q;
	
	for (int i=0; i<seedExteriorVoxels.size(); ++i)
	{
		int voxelIndex = seedExteriorVoxels[i];
		Q.push(voxelIndex);
	}
	
	while (Q.size() != 0)
	{
		int nextVoxelIndex = Q.front();
		Q.pop();
		
		Voxelization::Voxel& voxel = m_masterGrid[nextVoxelIndex];
		if (voxel.m_active) continue;
		
		voxel.m_exterior = true;
		
		// Add neighbouring voxels to the queue if they have not already been flagged as exterior, and are not active
		int ci, cj, ck;
		_voxelCoords(nextVoxelIndex, ci, cj, ck);
		
		// Do not allow diagonal moves here, otherwise there can be leaks through the boundary shell!			
		_testNeighbour_floodFillExteriorVoxels(ci-1, cj, ck, Q);
		_testNeighbour_floodFillExteriorVoxels(ci+1, cj, ck, Q);
		_testNeighbour_floodFillExteriorVoxels(ci, cj-1, ck, Q);
		_testNeighbour_floodFillExteriorVoxels(ci, cj+1, ck, Q);
		_testNeighbour_floodFillExteriorVoxels(ci, cj, ck-1, Q);
		_testNeighbour_floodFillExteriorVoxels(ci, cj, ck+1, Q);
	}
}


void Voxelization::generateFractureSeeds()
{
	// The voxel is assigned a weight given by the average of the weights of the contained vertices.
	// Keep local Voxel array to do sorting by weight.
	vector<Voxel> seedVoxels;
	set<int> seedVoxelIndices;
	
	for (int vi=0; vi<m_referenceModel.m_geometry.m_vertices.size(); ++vi)
	{
		const Vec4& P = m_referenceModel.m_geometry.m_vertices[vi];
		
		int ci, cj, ck;
		_getVoxelContaining(P, ci, cj, ck);
		
		int voxelIndex = _voxelIndex(ci, cj, ck);
		Voxel& voxel = m_masterGrid[voxelIndex];
		
		if (!voxel.m_active) voxel.m_active = true; // should be active anyway, unless triangle intersection test failed
		
		Vec4 color = m_referenceModel.m_geometry.m_rgbvertexWeight[vi];
		
		// White color is the background - does not create a seed voxel.
		if ( color.length3()/sqrtf(3.f) > 1.f - 1.0e-3f ) continue;
		
		// Map RGB color to hue, which when sorted should segregate reasonably dissimilar colors reasonably well.
		float weight = _rgbToHue(color[0], color[1], color[2]);
		voxel.m_weight = weight;

		// Add it to array if not already added
		if (seedVoxelIndices.count(voxelIndex) == 0) 
		{
			seedVoxelIndices.insert(voxelIndex);
			seedVoxels.push_back(voxel);
		}
	}
	
	// Dump weights back into master grid
	for (int i=0; i<seedVoxels.size(); ++i)
	{
		Voxel& voxel = seedVoxels[i];
		m_masterGrid[voxel.m_voxelIndex] = voxel;
	}
	
	// Now sort the seed voxels by weight, and thus break into sufficiently-same-weight chunks - each of which is the seed for a fracture piece.
	std::sort(seedVoxels.begin(), seedVoxels.end(), _compareVoxelWeights);
	
	int chunkStart=0;
	
	while (chunkStart < seedVoxels.size())
	{
		int startVoxelIndex = seedVoxels[chunkStart].m_voxelIndex;
		Voxel& startVoxel = m_masterGrid[startVoxelIndex];
		
		float startWeight = startVoxel.m_weight;
		int chunkEnd;
		
		for (chunkEnd=chunkStart+1; chunkEnd<seedVoxels.size(); ++chunkEnd)
		{
			int endVoxelIndex = seedVoxels[chunkEnd].m_voxelIndex;
			Voxel& endVoxel = m_masterGrid[endVoxelIndex];
			
			float weight = endVoxel.m_weight;
			if (fabs(weight-startWeight) > m_weightMargin)
			{
				chunkEnd--;
				break;
			}
		}
		
		if (chunkEnd==seedVoxels.size()) chunkEnd--;
		
		FracturePiece fracturePiece;
		for (int c=chunkStart; c<=chunkEnd; ++c)
		{
			Voxel& seedVoxel = m_masterGrid[seedVoxels[c].m_voxelIndex];
			
			seedVoxel.m_fracturePiece = m_fracturePieces.size();
			fracturePiece.m_voxelIndices.push_back(seedVoxel.m_voxelIndex);
		}
		
		if (fracturePiece.m_voxelIndices.size()>0)
		{
			m_fracturePieces.push_back(fracturePiece);
		}
		
		chunkStart = chunkEnd+1;
	}	
}



void Voxelization::buildFracturePieces()
{
	printf("Building fracture pieces...\n");
	
	int numProcessedVoxels = 0;
	
	// Breadth first search works fine here, having seeded the queue with all the fracture pieces.
	// This corresponds to growing a single layer on each fracture piece successively.
	
	std::queue<int> Q;
	
	for (int i=0; i<m_fracturePieces.size(); ++i)
	{
		FracturePiece& piece = m_fracturePieces[i];
		
		for (int vi=0; vi<piece.m_voxelIndices.size(); ++vi)
		{
			int voxelIndex = piece.m_voxelIndices[vi];
			Q.push(voxelIndex);
			
			Voxel& voxel = m_masterGrid[voxelIndex];
			assert(voxel.m_fracturePiece>-1); // check seed voxels were all assigned to some fracture piece
			
			numProcessedVoxels++;
		}
	}
	
	while (Q.size() != 0)
	{
		int nextVoxelIndex = Q.front();
		Q.pop();
		
		Voxel& voxel = m_masterGrid[nextVoxelIndex];
		if (!voxel.m_active) continue; // just a check
		
		assert(voxel.m_fracturePiece>-1);
		FracturePiece& fracturePiece = m_fracturePieces[voxel.m_fracturePiece];
		
		// Add neighbouring voxels to the queue if they are a) active, and b) not already belonging to some other fracture piece
		int ci, cj, ck;
		_voxelCoords(nextVoxelIndex, ci, cj, ck);
		
		// (We allow diagonal moves here, otherwise we would not be able to reach all of the voxels)			
		for (int i=-1; i<=1; ++i)
		{
			int ni = ci+i;  if (ni<0 || ni>=m_cageResolution[0]) continue;
			for (int j=-1; j<=1; ++j)
			{
				int nj = cj+j; if (nj<0 || nj>=m_cageResolution[1]) continue;
				for (int k=-1; k<=1; ++k)
				{
					int nk = ck+k; if (nk<0 || nk>=m_cageResolution[2]) continue;
					if (i==0 && j==0 && k==0) continue;
					
					int neighbourVoxelIndex = _voxelIndex(ni, nj, nk);
					Voxelization::Voxel& neighbourvoxel = m_masterGrid[neighbourVoxelIndex];
					
					if ( neighbourvoxel.m_active && neighbourvoxel.m_fracturePiece<0 )
					{
						neighbourvoxel.m_fracturePiece = voxel.m_fracturePiece;
						fracturePiece.m_voxelIndices.push_back(neighbourVoxelIndex);
						Q.push(neighbourVoxelIndex);
						
						numProcessedVoxels++;
						
						if (numProcessedVoxels % 100000 == 0)
						{
							printf("     processed %d/%d voxels\n", numProcessedVoxels, m_numActiveVoxels);
						}
					}
				}
			}
		}
	}
}


int Voxelization::_cornerVertex(const Voxelization::Corner corner) const
{
	return _latticeIndex(corner.m_latticeCoord[0], corner.m_latticeCoord[1], corner.m_latticeCoord[2]);
}

void Voxelization::_addVoxelFace(int ci, int cj, int ck, const Corner corners[8], vector<Geometry::Triangle>& triangles) const
{
	int va = _cornerVertex(corners[ci]);
	int vb = _cornerVertex(corners[cj]);
	int vc = _cornerVertex(corners[ck]);
	
	// Create raw geometry - no normals or UVs yet!
	Geometry::Triangle triangle;
	triangle.m_vertex[0] = va;
	triangle.m_vertex[1] = vb;
	triangle.m_vertex[2] = vc;
	triangles.push_back(triangle);
}

bool Voxelization::_adjacentVoxelInFracturePiece(int ci, int cj, int ck, int dci, int dcj, int dck, int fracturePieceIndex)
{
	int ciAdj = ci + dci;
	if (ciAdj<0 || ciAdj>=m_cageResolution[0]) return false;
	
	int cjAdj = cj + dcj;
	if (cjAdj<0 || cjAdj>=m_cageResolution[1]) return false;
	
	int ckAdj = ck + dck;
	if (ckAdj<0 || ckAdj>=m_cageResolution[3]) return false;
	
	Voxel& adjVoxel = m_masterGrid[_voxelIndex(ciAdj, cjAdj, ckAdj)];
	
	return adjVoxel.m_fracturePiece == fracturePieceIndex;
}

void Voxelization::generateMeshes()
{
	for (int fracturePieceIndex=0; fracturePieceIndex<m_fracturePieces.size(); ++fracturePieceIndex)
	{
		printf("Generating initial mesh for fracture piece %d/%d...\n", fracturePieceIndex+1, (int)m_fracturePieces.size());
		FracturePiece& piece = m_fracturePieces[fracturePieceIndex];
		
		// Our job is to fill this array of triangles, given the fracture piece voxels
		vector<Geometry::Triangle>& triangles = piece.m_triangles;
		
		// Go through all voxels
		for (int vi=0; vi<piece.m_voxelIndices.size(); ++vi)
		{
			int voxelIndex = piece.m_voxelIndices[vi];
			
			int ci, cj, ck;
			_voxelCoords(voxelIndex, ci, cj, ck);
			
			Voxelization::Corner corners[8];
			for (int corner=0; corner<8; ++corner)
			{
				int vi = ci + ((corner >> 0) & 1);
				int vj = cj + ((corner >> 1) & 1);
				int vk = ck + ((corner >> 2) & 1);		
				corners[corner].m_latticeCoord[0] = vi;
				corners[corner].m_latticeCoord[1] = vj;
				corners[corner].m_latticeCoord[2] = vk;
			}
			
			// Make a pair of triangles for each voxel face which adjoins either the lattice boundary or another fracture piece
			if ( !_adjacentVoxelInFracturePiece(ci, cj, ck, 0, 0, -1, fracturePieceIndex) )
			{
				_addVoxelFace(0, 3, 1, corners, triangles); //-Z
				_addVoxelFace(0, 2, 3, corners, triangles);
			}
			if ( !_adjacentVoxelInFracturePiece(ci, cj, ck, 0, 0, 1, fracturePieceIndex) )
			{
				_addVoxelFace(4, 5, 7, corners, triangles); //+Z
				_addVoxelFace(4, 7, 6, corners, triangles);
			}
			if ( !_adjacentVoxelInFracturePiece(ci, cj, ck, -1, 0, 0, fracturePieceIndex) )
			{
				_addVoxelFace(0, 6, 2, corners, triangles); //-X
				_addVoxelFace(0, 4, 6, corners, triangles);
			}
			if ( !_adjacentVoxelInFracturePiece(ci, cj, ck, 1, 0, 0, fracturePieceIndex) )
			{
				_addVoxelFace(1, 3, 7, corners, triangles); //+X
				_addVoxelFace(1, 7, 5, corners, triangles);
			}
			if ( !_adjacentVoxelInFracturePiece(ci, cj, ck, 0, -1, 0, fracturePieceIndex) )
			{
				_addVoxelFace(0, 1, 5, corners, triangles); //-Y
				_addVoxelFace(0, 5, 4, corners, triangles);
			}
			if ( !_adjacentVoxelInFracturePiece(ci, cj, ck, 0, 1, 0, fracturePieceIndex) )
			{
				_addVoxelFace(2, 7, 3, corners, triangles); //+Y
				_addVoxelFace(2, 6, 7, corners, triangles);
			}
			
		} // voxels of fracture piece
		
	} // fracture piece
}


void Voxelization::projectOntoReferenceMesh()
{
	Vec4 cullWidth(m_voxelEdges[0], m_voxelEdges[1], m_voxelEdges[2]);
	cullWidth *= 3.0f;
	
	int boundaryIndex = 0;
	float nextFraction = 0.05f;
	
	for( set<int>::iterator iter = m_boundaryVertices.begin(); iter != m_boundaryVertices.end(); iter++)
	{
		float fractionDone = float(boundaryIndex) / float(m_boundaryVertices.size()-1);
		if (fractionDone>nextFraction)
		{
			printf("     done %2.0f percent of boundary...\n", fractionDone*100.f);
			nextFraction += 0.05f;
		}
		
		int latticeIndex = *iter;
		
		int vi, vj, vk;
		_vertexCoords(latticeIndex, vi, vj, vk);
		
		Vec4 P = _latticeVertex(vi, vj, vk);
		
		// Find geometrically closest mesh triangle to this point, and project point onto it.
		
		// Cull all triangles further than 3 voxel widths away from point.
		Aabb cullingAabb;
		cullingAabb.includePoint(P);
		cullingAabb.m_maxima += cullWidth;
		cullingAabb.m_minima -= cullWidth;
		
		vector<int> triangleIndices;
		m_meshKdTree->intersect(cullingAabb, triangleIndices);
		
		float minDist = 1.0e10f;
		int closestTriIndex = -1;
		
		for (int ti=0; ti<triangleIndices.size(); ++ti)
		{
			int triIndex = triangleIndices[ti];
			const Geometry::Triangle& triangle = m_referenceModel.m_geometry.m_triangles[triIndex];
			
			const Vec4& A = m_referenceModel.m_geometry.m_vertices[triangle.m_vertex[0]];
			const Vec4& B = m_referenceModel.m_geometry.m_vertices[triangle.m_vertex[1]];
			const Vec4& C = m_referenceModel.m_geometry.m_vertices[triangle.m_vertex[2]];
			
			float d = GeometryUtils::vertexTriangleDistance(P, A, B, C);
			
			if (d<minDist)
			{
				minDist = d;
				closestTriIndex = triIndex;
			}
		}
		
		if (closestTriIndex<0)
		{
			printf("Unable to snap boundary vertex %d\n", latticeIndex);
			m_boundaryUVs.insert(map<int, Vec4>::value_type(latticeIndex, Vec4(0,0,0)));
			m_boundaryNormals.insert(map<int, Vec4>::value_type(latticeIndex, Vec4(0,0,0)));
			continue;
		}
		
		const Geometry::Triangle& closestTriangle = m_referenceModel.m_geometry.m_triangles[closestTriIndex];
		{
			const Vec4& A = m_referenceModel.m_geometry.m_vertices[closestTriangle.m_vertex[0]];
			const Vec4& B = m_referenceModel.m_geometry.m_vertices[closestTriangle.m_vertex[1]];
			const Vec4& C = m_referenceModel.m_geometry.m_vertices[closestTriangle.m_vertex[2]];
			
			// Move boundary vertex to the closest point on the mesh
			Vec4 X = GeometryUtils::getClosestPointOnTriangle(P, A, B, C);
			
			// Use barycoords to interpolate the triangle UVs and normals, and assign them to the vertex.
			float w[3];
			GeometryUtils::getProjectedBarycoords(X, A, B, C, w);
			
			const Vec4& uvA = m_referenceModel.m_geometry.m_UVs[closestTriangle.m_uvs[0]];
			const Vec4& uvB = m_referenceModel.m_geometry.m_UVs[closestTriangle.m_uvs[1]];
			const Vec4& uvC = m_referenceModel.m_geometry.m_UVs[closestTriangle.m_uvs[2]];
			
			Vec4 uvI = w[0]*uvA + w[1]*uvB + w[2]*uvC;
			m_boundaryUVs.insert(map<int, Vec4>::value_type(latticeIndex, uvI));
			
			const Vec4& NA = m_referenceModel.m_geometry.m_vertexNormals[closestTriangle.m_vertexNormal[0]];
			const Vec4& NB = m_referenceModel.m_geometry.m_vertexNormals[closestTriangle.m_vertexNormal[1]];
			const Vec4& NC = m_referenceModel.m_geometry.m_vertexNormals[closestTriangle.m_vertexNormal[2]];
			
			Vec4 NI = w[0]*NA + w[1]*NB + w[2]*NC;
			NI.normalizeIfNotZero();
			m_boundaryNormals.insert(map<int, Vec4>::value_type(latticeIndex, NI));
			
			_setLatticeVertex(latticeIndex, X);
		}
		
		boundaryIndex++;
	}	
}


// Compute the position of a given lattice point given its lattice indices
Vec4 Voxelization::_latticeVertex(int vi, int vj, int vk) const
{
	return m_masterVertices[ _latticeIndex(vi, vj, vk) ];
}

// Update the lattice positions
void Voxelization::_setLatticeVertex(int latticeIndex, const Vec4& P)
{
	m_masterVertices[latticeIndex] = P;
}

// Return index into vertex array, given vertex lattice coords each in range [0, m_cageResolution[i]]
int Voxelization::_latticeIndex(int vi, int vj, int vk) const
{
	return vk + (m_cageResolution[2]+1)*(vj + (m_cageResolution[1]+1)*vi);
}

// Return voxel index (into the master grid array), given voxel indices
int Voxelization::_voxelIndex(int ci, int cj, int ck) const
{
	return ck + m_cageResolution[2]*(cj + m_cageResolution[1]*ci);
}

// Given the lattice index of a lattice point, return its lattice indices
void Voxelization::_vertexCoords(int latticeIndex, int &vi, int &vj, int& vk) const
{
	vk = latticeIndex % (m_cageResolution[2]+1);
	int m = (latticeIndex - vk)/(m_cageResolution[2]+1);
	vj = m % (m_cageResolution[1]+1);
	vi = (m - vj)/(m_cageResolution[1]+1);
}

// Given the index of a voxel into the master grid, return its voxel indices
void Voxelization::_voxelCoords(int voxelIndex, int& ci, int&cj, int& ck) const
{
	ck = voxelIndex % m_cageResolution[2];
	int m = (voxelIndex - ck)/m_cageResolution[2];
	cj = m % m_cageResolution[1];
	ci = (m - cj)/m_cageResolution[1];
}

// Return voxel indices each in range of the voxel containing a given point 
inline void Voxelization::_getVoxelContaining(const Vec4& point, int& ci, int& cj, int& ck) const
{
	ci = static_cast<int>( (point[0]-m_meshAabb.m_minima[0])/m_voxelEdges[0] ); if (ci == m_cageResolution[0]) ci -= 1; if (ci<0) ci=0;
	cj = static_cast<int>( (point[1]-m_meshAabb.m_minima[1])/m_voxelEdges[1]);  if (cj == m_cageResolution[1]) cj -= 1; if (cj<0) cj=0;
	ck = static_cast<int>( (point[2]-m_meshAabb.m_minima[2])/m_voxelEdges[2]);  if (ck == m_cageResolution[2]) ck -= 1; if (ck<0) ck=0;
}

// Compute the AABB of a given voxel, given its voxel indices
Aabb Voxelization::_computeVoxelAabb(int ci, int cj, int ck) const
{
	Aabb aabb;
	for (int corner=0; corner<8; ++corner)
	{
		aabb.includePoint( getCornerVertex(corner, ci, cj, ck) );
	}
	return aabb;
}

Vec4 Voxelization::getCornerVertex(int corner, int ci, int cj, int ck) const
{
	int vi = ci + ((corner >> 0) & 1);
	int vj = cj + ((corner >> 1) & 1);
	int vk = ck + ((corner >> 2) & 1);
	return _latticeVertex(vi, vj, vk);
}








